/* ============================================================================
 * Freetype GL - A C OpenGL Freetype engine
 * Platform:    Any
 * WWW:         http://code.google.com/p/freetype-gl/
 * ----------------------------------------------------------------------------
 * Copyright 2011,2012 Nicolas P. Rougier. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  1. Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY NICOLAS P. ROUGIER ''AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO
 * EVENT SHALL NICOLAS P. ROUGIER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of Nicolas P. Rougier.
 * ============================================================================
 */
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "mat4.h"

#ifndef M_PI
#    define M_PI 3.14159265358979323846
#endif

mat4 *
mat4_new( void )
{
    mat4 *self = (mat4 *) malloc( sizeof(mat4) );
    return self;

}

void
mat4_set_zero( mat4 *self )
{
    assert( self );

    memset( self, 0, sizeof( mat4 ));
}

void
mat4_set_identity( mat4 *self )
{
    assert( self );

    memset( self, 0, sizeof( mat4 ));
    self->m00 = 1.0;
    self->m11 = 1.0;
    self->m22 = 1.0;
    self->m33 = 1.0;
}

void
mat4_multiply( mat4 *self, mat4 *other )
{
    mat4 m;
    size_t i;

    assert( self );
    assert( other );

    for( i=0; i<4; ++i )
    {
        m.data[i*4+0] =
            (self->data[i*4+0] * other->data[0*4+0]) +
            (self->data[i*4+1] * other->data[1*4+0]) +
            (self->data[i*4+2] * other->data[2*4+0]) +
            (self->data[i*4+3] * other->data[3*4+0]) ;

        m.data[i*4+1] =
            (self->data[i*4+0] * other->data[0*4+1]) +
            (self->data[i*4+1] * other->data[1*4+1]) +
            (self->data[i*4+2] * other->data[2*4+1]) +
            (self->data[i*4+3] * other->data[3*4+1]) ;
        
        m.data[i*4+2] =
            (self->data[i*4+0] * other->data[0*4+2]) +
            (self->data[i*4+1] * other->data[1*4+2]) +
            (self->data[i*4+2] * other->data[2*4+2]) +
            (self->data[i*4+3] * other->data[3*4+2]) ;
        
        m.data[i*4+3] =
            (self->data[i*4+0] * other->data[0*4+3]) +
            (self->data[i*4+1] * other->data[1*4+3]) +
            (self->data[i*4+2] * other->data[2*4+3]) +
            (self->data[i*4+3] * other->data[3*4+3]) ;
    }
    memcpy( self, &m, sizeof( mat4 ) );

}


void
mat4_set_orthographic( mat4 *self,
                       float left,   float right,
                       float bottom, float top,
                       float znear,  float zfar )
{
    assert( self );
    assert( right  != left );
    assert( bottom != top  );
    assert( znear  != zfar );

    mat4_set_zero( self );

    self->m00 = +2.0/(right-left);
    self->m30 = -(right+left)/(right-left);
    self->m11 = +2.0/(top-bottom);
    self->m31 = -(top+bottom)/(top-bottom);
    self->m22 = -2.0/(zfar-znear);
    self->m32 = -(zfar+znear)/(zfar-znear);
    self->m33 = 1.0;
}

void
mat4_set_perspective( mat4 *self,
                      float fovy,  float aspect,
                      float znear, float zfar)
{
    float h, w;
   
    assert( self );
    assert( znear != zfar );

    h = tan(fovy / 360.0 * M_PI) * znear;
    w = h * aspect;
 
    mat4_set_frustum( self, -w, w, -h, h, znear, zfar );
}

void
mat4_set_frustum( mat4 *self,
                  float left,   float right,
                  float bottom, float top,
                  float znear,  float zfar )
{
    
    assert( self );
    assert( right  != left );
    assert( bottom != top  );
    assert( znear  != zfar );

    mat4_set_zero( self );

    self->m00 = +2.0*znear/(right-left);
    self->m20 = (right+left)/(right-left);

    self->m11 = +2.0*znear/(top-bottom);
    self->m31 = (top+bottom)/(top-bottom);

    self->m22 = -(zfar+znear)/(zfar-znear);
    self->m32 = -2.0*znear/(zfar-znear);

    self->m23 = -1.0;
}

void
mat4_set_rotation( mat4 *self,
                   float angle,
                   float x, float y, float z)
{
    float c, s, norm;
  
    assert( self );

    c = cos( M_PI*angle/180.0 );
    s = sin( M_PI*angle/180.0 );
    norm = sqrt(x*x+y*y+z*z);

    x /= norm; y /= norm; z /= norm;

    mat4_set_identity( self );

    self->m00 = x*x*(1-c)+c;
    self->m10 = y*x*(1-c)-z*s;
    self->m20 = z*x*(1-c)+y*s;

    self->m01 =  x*y*(1-c)+z*s;
    self->m11 =  y*y*(1-c)+c;
    self->m21 =  z*y*(1-c)-x*s;

    self->m02 = x*z*(1-c)-y*s;
    self->m12 = y*z*(1-c)+x*s;
    self->m22 = z*z*(1-c)+c;
}

void
mat4_set_translation( mat4 *self,
                      float x, float y, float z)
{
    assert( self );

    mat4_set_identity( self );
    self-> m30 = x;
    self-> m31 = y;
    self-> m32 = z;
}

void
mat4_set_scaling( mat4 *self,
                  float x, float y, float z)
{
    assert( self );

    mat4_set_identity( self );
    self-> m00 = x;
    self-> m11 = y;
    self-> m22 = z;
}

void
mat4_rotate( mat4 *self,
             float angle,
             float x, float y, float z)
{
    mat4 m;
    
    assert( self );

    mat4_set_rotation( &m, angle, x, y, z);
    mat4_multiply( self, &m );
}

void
mat4_translate( mat4 *self,
                float x, float y, float z)
{
    mat4 m;
    assert( self );

    mat4_set_translation( &m, x, y, z);
    mat4_multiply( self, &m );
}

void
mat4_scale( mat4 *self,
            float x, float y, float z)
{
    mat4 m;
    assert( self );

    mat4_set_scaling( &m, x, y, z);
    mat4_multiply( self, &m );
}

//=============================================================================================
// gluPerspective implementation  
void  
mat4_set_perspective_x(mat4 *self, float fovy, float aspect, float zNear, float zFar)  
{  
	float f = 0;
	float *projMatrix = self->data;

	assert(self);
	mat4_set_identity(self);

	f = 1.0f / tan(fovy * (M_PI / 360.0f));
	projMatrix[0] = f / aspect;
	projMatrix[1 * 4 + 1] = f;  
	projMatrix[2 * 4 + 2] = (zFar + zNear) / (zNear - zFar);  
	projMatrix[3 * 4 + 2] = (2.0f * zFar * zNear) / (zNear - zFar);  
	projMatrix[2 * 4 + 3] = -1.0f;  
	projMatrix[3 * 4 + 3] = 0.0f;  
}  

void
mat4_set_frustum_x(mat4 *self, float left, float right, float bottom, float top, float nearp, float farp)
{
	float* m = 0;
	assert(self);

	mat4_set_identity(self);

	m = self->data;
	m[0 * 4 + 0] = 2 * nearp / (right-left);  
	m[1 * 4 + 1] = 2 * nearp / (top - bottom);  
	m[2 * 4 + 0] = (right + left) / (right - left);  
	m[2 * 4 + 1] = (top + bottom) / (top - bottom);  
	m[2 * 4 + 2] = - (farp + nearp) / (farp - nearp);  
	m[2 * 4 + 3] = -1.0f;  
	m[3 * 4 + 2] = - 2 * farp * nearp / (farp-nearp);  
	m[3 * 4 + 3] = 0.0f;
}

void private_cross_product(float *a, float *b, float *res) 
{  
	res[0] = a[1] * b[2]  -  b[1] * a[2];  
	res[1] = a[2] * b[0]  -  b[2] * a[0];  
	res[2] = a[0] * b[1]  -  b[0] * a[1];  
}  

// Normalize a vec3  
void private_normalize(float *a) 
{  
	float mag = sqrt(a[0] * a[0]  +  a[1] * a[1]  +  a[2] * a[2]);  
	a[0] /= mag;  
	a[1] /= mag;  
	a[2] /= mag;  
}  

void 
mat4_look_at(mat4 *self, float xPos, float yPos, float zPos, 
			 float xLook, float yLook, float zLook, 
			 float xUp, float yUp, float zUp)
{
	float dir[3], right[3], up[3];  
	mat4 mat1;
	float *m1 = 0;
	float *m2 = 0;

	assert(self);

	mat4_set_identity(&mat1);
	mat4_set_identity(self);

	up[0] = xUp;    up[1] = yUp;    up[2] = zUp;  

	dir[0] =  (xLook - xPos);  
	dir[1] =  (yLook - yPos);  
	dir[2] =  (zLook - zPos);  
	private_normalize(dir);

	private_cross_product(dir,up,right);  
	private_normalize(right);

	private_cross_product(right,dir,up);  
	private_normalize(up);

	m1 = self->data;
	m2 = mat1.data;

	m1[0]  = right[0];  
	m1[4]  = right[1];  
	m1[8]  = right[2];  
	m1[12] = 0.0f;  

	m1[1]  = up[0];  
	m1[5]  = up[1];  
	m1[9]  = up[2];  
	m1[13] = 0.0f;  

	m1[2]  = -dir[0];  
	m1[6]  = -dir[1];  
	m1[10] = -dir[2];  
	m1[14] =  0.0f;  

	m1[3]  = 0.0f;  
	m1[7]  = 0.0f;  
	m1[11] = 0.0f;  
	m1[15] = 1.0f;  

	m2[12] = -xPos;
	m2[13] = -yPos;
	m2[14] = -zPos;

	mat4_multiply(self, &mat1);	
}